
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       sensor_compass_backend.c
  * @author     baiyang
  * @date       2021-11-24
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "sensor_compass.h"
#include "sensor_compass_backend.h"

#include <string.h>
#include <float.h>

#include <uITC/uITC.h>
#include <uITC/uITC_msg.h>
#include <parameter/param.h>
#include <common/time/gp_time.h>
#include <common/gp_math/gp_mathlib.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/
static void compass_backend_destructor(sensor_compass_backend *compass_backend);
static void compass_backend_read(sensor_compass_backend *compass_backend);
/*----------------------------------variable----------------------------------*/
uitc_sensor_mag sensor_mag;

// access to frontend
static sensor_compass *_compass;
static const float FILTER_KOEF = 0.1f;

struct sensor_compass_backend_ops compass_backend_ops = {.compass_backend_destructor = compass_backend_destructor,
                                                         .read = compass_backend_read};
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
static void compass_backend_destructor(sensor_compass_backend *compass_backend) { rt_mutex_delete(compass_backend->_mutex); }
static void compass_backend_read(sensor_compass_backend *compass_backend){}

void sensor_compass_backend_ctor(sensor_compass_backend *compass_backend, char *name)
{
    // 清空sensor_imu_backend结构体变量，因为sensor_imu_backend结构体有可能是申请的动态内存
    // 防止sensor_imu_backend中的指针变量初始为非零值。
    memset(compass_backend, 0, sizeof(sensor_compass_backend));

    _compass = sensor_compass_get_singleton();

    compass_backend->ops = &compass_backend_ops;

    compass_backend->_mutex = rt_mutex_create(name, RT_IPC_FLAG_FIFO);
}

void sensor_compass_backend_rotate_field(Vector3f_t *mag, uint8_t instance)
{
    mag_state *state = &_compass->_state[instance];
    if (MAG_BOARD_ORIENTATION != ROTATION_NONE) {
        vec3_rotate(mag, MAG_BOARD_ORIENTATION);
    }

    vec3_rotate(mag, state->rotation);

    if (!state->external) {
        // and add in AHRS_ORIENTATION setting if not an external compass
        if (_compass->_board_orientation == ROTATION_CUSTOM) {
            matrix3f_mul_vec(mag, _compass->_custom_rotation, mag);
        } else {
            vec3_rotate(mag, _compass->_board_orientation);
        }
    } else {
        // add user selectable orientation
        enum RotationEnum rotation = state->orientation;
        if (rotation == ROTATION_CUSTOM && _compass->_custom_external_rotation != NULL) {
            matrix3f_mul_vec(mag, _compass->_custom_external_rotation, mag);
        } else {
            vec3_rotate(mag, rotation);
        }
    }

    if (instance == 0) {
        sensor_mag.timestamp_us = time_micros64();
        sensor_mag.sensor_mag_measure[0] = mag->x * 0.001f;  //milligauss -> gauss
        sensor_mag.sensor_mag_measure[1] = mag->y * 0.001f;
        sensor_mag.sensor_mag_measure[2] = mag->z * 0.001f;
    }
}

void sensor_compass_backend_publish_raw_field(const Vector3f_t *mag, uint8_t instance)
{
    // note that we do not set last_update_usec here as otherwise the
    // EKF and DCM would end up consuming compass data at the full
    // sensor rate. We want them to consume only the filtered fields
#if COMPASS_CAL_ENABLED
    CompassCalibrator* cal = _compass->_calibrator[sensor_compass_get_priority(instance)];
    if (cal != NULL) {
        mag_state* state = &_compass->_state[instance];
        state->last_update_ms = time_millis();
        compasscal_new_sample(cal, mag);
    }
#endif
}

void sensor_compass_backend_correct_field(Vector3f_t *mag, uint8_t i)
{
    mag_state *state = &_compass->_state[i];

    const Vector3f_t *offsets = &state->offset;
    const Vector3f_t *diagonals = &state->diagonals;
    const Vector3f_t *offdiagonals = &state->offdiagonals;

    // add in the basic offsets
    //mag += offsets;
    vec3_add(mag, mag, offsets);

    // add in scale factor, use a wide sanity check. The calibrator
    // uses a narrower check.
    if (sensor_compass_have_scale_factor(i)) {
        //mag *= state->scale_factor;
        vec3_mult(mag, mag, state->scale_factor);
    }

    // apply eliptical correction
    float mat[3][3] = {
        diagonals->x, offdiagonals->x, offdiagonals->y,
        offdiagonals->x,    diagonals->y, offdiagonals->z,
        offdiagonals->y, offdiagonals->z,    diagonals->z
    };

    //mag = mat * mag;
    float temp[3];
    temp[0] = mag->x;
    temp[1] = mag->y;
    temp[2] = mag->z;
    
    mag->vec3[0] = mat[0][0] * temp[0] + mat[0][1] * temp[1] + mat[0][2] * temp[2];
    mag->vec3[1] = mat[1][0] * temp[0] + mat[1][1] * temp[1] + mat[1][2] * temp[2];
    mag->vec3[2] = mat[2][0] * temp[0] + mat[2][1] * temp[1] + mat[2][2] * temp[2];

#if 0 //COMPASS_MOT_ENABLED
    const Vector3f &mot = state.motor_compensation.get();
    /*
      calculate motor-power based compensation
      note that _motor_offset[] is kept even if compensation is not
      being applied so it can be logged correctly
    */    
    state.motor_offset.zero();
    if (_compass._per_motor.enabled() && i == 0) {
        // per-motor correction is only valid for first compass
        _compass._per_motor.compensate(state.motor_offset);
    } else if (_compass._motor_comp_type == AP_COMPASS_MOT_COMP_THROTTLE) {
        state.motor_offset = mot * _compass._thr;
    } else if (_compass._motor_comp_type == AP_COMPASS_MOT_COMP_CURRENT) {
        AP_BattMonitor &battery = AP::battery();
        float current;
        if (battery.current_amps(current)) {
            state.motor_offset = mot * current;
        }
    }

    /*
      we apply the motor offsets after we apply the eliptical
      correction. This is needed to match the way that the motor
      compensation values are calculated, as they are calculated based
      on final field outputs, not on the raw outputs
    */
    mag += state.motor_offset;
#endif // COMPASS_MOT_ENABLED

    if (i == 0) {
        sensor_mag.sensor_mag_correct[0] = mag->x * 0.001f;  //milligauss -> gauss
        sensor_mag.sensor_mag_correct[1] = mag->y * 0.001f;
        sensor_mag.sensor_mag_correct[2] = mag->z * 0.001f;

        itc_publish(ITC_ID(sensor_mag), &sensor_mag);
    }
}

void sensor_compass_backend_accumulate_sample(sensor_compass_backend *compass_backend, Vector3f_t *field, uint8_t instance,
                                           uint32_t max_samples)
{
    /* rotate raw_field from sensor frame to body frame */
    sensor_compass_backend_rotate_field(field, instance);

    /* publish raw_field (uncorrected point sample) for calibration use */
    sensor_compass_backend_publish_raw_field(field, instance);

    /* correct raw_field for known errors */
    sensor_compass_backend_correct_field(field, instance);

    if (!sensor_compass_backend_field_ok(compass_backend, field)) {
        return;
    }

    rt_mutex_take(compass_backend->_mutex, RT_WAITING_FOREVER);

    mag_state *state = &_compass->_state[instance];
    //state->accum += field;
    vec3_add(&state->accum, &state->accum, field);
    state->accum_count++;
    if (max_samples && state->accum_count >= max_samples) {
        state->accum_count /= 2;
        //state->accum /= 2;
        vec3_mult(&state->accum, &state->accum, 0.5f);
    }

    rt_mutex_release(compass_backend->_mutex);
}

void sensor_compass_backend_drain_accumulated_samples(sensor_compass_backend *compass_backend, uint8_t instance, const Vector3f_t *scaling)
{
    rt_mutex_take(compass_backend->_mutex, RT_WAITING_FOREVER);

    mag_state *state = &_compass->_state[instance];

    if (state->accum_count == 0) {
       rt_mutex_release(compass_backend->_mutex);
       return;
    }

    if (scaling) {
       vec3_mult_scalar_vec(&state->accum, &state->accum, scaling);
    }
    //state->accum /= state->accum_count;
    vec3_mult(&state->accum, &state->accum, 1.0f/state->accum_count);

    sensor_compass_backend_publish_filtered_field(&state->accum, instance);

    vec3_zero(&state->accum);
    state->accum_count = 0;

    rt_mutex_release(compass_backend->_mutex);
}

/*
  copy latest data to the frontend from a backend
 */
void sensor_compass_backend_publish_filtered_field(const Vector3f_t *mag, uint8_t instance)
{
    mag_state *state = &_compass->_state[instance];

    state->field = *mag;

    state->last_update_ms = time_millis();
    state->last_update_usec = time_micros64();
}

void sensor_compass_backend_set_last_update_usec(uint64_t last_update, uint8_t instance)
{
    mag_state *state = &_compass->_state[instance];
    state->last_update_usec = last_update;
}

/*
  register a new backend with frontend, returning instance which
  should be used in publish_field()
 */
bool sensor_compass_backend_register_compass(int32_t dev_id, uint8_t* instance)
{ 
    return sensor_compass_register_compass(dev_id, instance);
}


/*
  set dev_id for an instance
*/
void sensor_compass_backend_set_dev_id(uint8_t instance, uint32_t dev_id)
{
    _compass->_state[instance].detected_dev_id = dev_id;

    if (instance == 0) {
        param_set_obj_and_notify(PARAM_ID(COMPASS, COMPASS_DEV_ID), dev_id);
    } else if (instance == 1) {
        param_set_obj_and_notify(PARAM_ID(COMPASS, COMPASS_DEV_ID2), dev_id);
    } else if (instance == 2) {
        param_set_obj_and_notify(PARAM_ID(COMPASS, COMPASS_DEV_ID3), dev_id);
    }
}

/*
  save dev_id, used by SITL compass_backend_undone
*/
void sensor_compass_backend_save_dev_id(uint8_t instance)
{
    if (instance == 0) {
        param_save_obj(PARAM_ID(COMPASS, COMPASS_DEV_ID));
    } else if (instance == 1) {
        param_save_obj(PARAM_ID(COMPASS, COMPASS_DEV_ID2));
    } else if (instance == 2) {
        param_save_obj(PARAM_ID(COMPASS, COMPASS_DEV_ID3));
    }
}

/*
  set external for an instanc
*/
void sensor_compass_backend_set_external(uint8_t instance, bool external)
{
    if (_compass->_state[instance].external != 2) {
        if (instance == 0) {
            param_set_obj_and_notify(PARAM_ID(COMPASS, COMPASS_EXTERNAL), external);
        } else if (instance == 1) {
            param_set_obj_and_notify(PARAM_ID(COMPASS, COMPASS_EXTERN2), external);
        } else if (instance == 2) {
            param_set_obj_and_notify(PARAM_ID(COMPASS, COMPASS_EXTERN3), external);
        }
    }
}

bool sensor_compass_backend_is_external(uint8_t instance)
{
    return _compass->_state[instance].external;
}

// set rotation of an instance 
void sensor_compass_backend_set_rotation(uint8_t instance, enum RotationEnum rotation)
{
    _compass->_state[instance].rotation = rotation;

    // lazily create the custom rotation matrix
    if (!_compass->_custom_external_rotation && (enum RotationEnum )(_compass->_state[instance].orientation) == ROTATION_CUSTOM) {
        _compass->_custom_external_rotation = (matrix3f_t *)rt_malloc(sizeof(matrix3f_t));
        if (_compass->_custom_external_rotation) {
            matrix3f_from_euler(_compass->_custom_external_rotation, radians(_compass->_custom_roll), radians(_compass->_custom_pitch), radians(_compass->_custom_yaw));
        }
    }
}

/* Check that the compass value is valid by using a mean filter. If
* the value is further than filtrer_range from mean value, it is
* rejected. 
*/
bool sensor_compass_backend_field_ok(sensor_compass_backend *compass_backend, const Vector3f_t *field)
{
    if (vec3_is_inf(field) || vec3_is_nan(field)) {
       return false;
    }

    if (_compass->_filter_range <= 0) {
       return true;
    }

    const float length = vec3_length(field);

    if (math_flt_zero(compass_backend->_mean_field_length)) {
       compass_backend->_mean_field_length = length;
       return true;
    }

    bool ret = true;
    const float d = fabsf(compass_backend->_mean_field_length - length) / (compass_backend->_mean_field_length + length);  // diff divide by mean value in percent ( with the *200.0f on later line)
    float koeff = FILTER_KOEF;

    if (d * 200.0f > _compass->_filter_range) {  // check the difference from mean value outside allowed range
       // printf("\nCompass field length error: mean %f got %f\n", (double)_mean_field_length, (double)length );
       ret = false;
       koeff /= (d * 10.0f);  // 2.5 and more, so one bad sample never change mean more than 4%
       compass_backend->_error_count++;
    }
    compass_backend->_mean_field_length = compass_backend->_mean_field_length * (1 - koeff) + length * koeff;  // complimentary filter 1/k

    return ret;
}


enum RotationEnum sensor_compass_backend_get_board_orientation(void)
{
    return _compass->_board_orientation;
}

/*------------------------------------test------------------------------------*/


